// 零差云控的实现
#include "eDriver.h"
//#include "innfos.h"

static eDriverMaster_t  eDriver;
static servo_sem_t  servo_conf[4];

// 读取的状态和位置的取值
static UNS16	status_test;
//static UNS32	position_test;


int MInitialize_eDriver(void)
{
    eDriver.agv_board.busname = "0";
    eDriver.agv_board.baudrate = "1M";
    eDriver.semTable = servo_conf;
    eDriver.od_Data = &master402_Data;
    eDriver.MSetSYNC = MSetSYNC;

    return MCanopen_init();
}
//INIT_APP_EXPORT(MInitialize_eDriver);

static int MCanopen_init(void)
{
    // 注册canopen相关回调函数
	eDriver.od_Data->heartbeatError = master402_heartbeatError;
	eDriver.od_Data->initialisation = master402_initialisation;
	eDriver.od_Data->preOperational = master402_preOperational;
	eDriver.od_Data->operational = master402_operational;
	eDriver.od_Data->stopped   = master402_stopped;
	eDriver.od_Data->post_sync = master402_post_sync;
	eDriver.od_Data->post_TPDO = master402_post_TPDO;
	eDriver.od_Data->storeODSubIndex = (storeODSubIndex_t)master402_storeODSubIndex;
	eDriver.od_Data->post_emcy = (post_emcy_t)master402_post_emcy;

    //	打开主站can接收数据线程
	canOpen(&eDriver.agv_board, eDriver.od_Data);
	//	初始化定时器
    initTimer();

	//	定时器倒计时启动主站节点初始化
	StartTimerLoop(&MInitNodes);
	
	return 0;
}

//初始化主站节点
void MInitNodes(CO_Data* d, UNS32 id)
{
	setNodeId(eDriver.od_Data, 0x01);
	setState(eDriver.od_Data, Initialisation);
}

// 停止主站
void MInitNodeExit(CO_Data* d, UNS32 id)
{
    masterSendNMTstateChange(eDriver.od_Data, ALL_SERVO_NODE, NMT_Reset_Node);
	setState(eDriver.od_Data, Stopped);
}

// 从站自启线程
static void slaveBootupHdl(CO_Data* d, UNS8 nodeId)
{
	rt_thread_t tid;

	tid = rt_thread_create("Servo_cfg", MConfig_Single_Servo, (void *)(int)nodeId, 1024, 12 + nodeId, 2);
	if(tid == RT_NULL)
	{
		ERROR_DEBUG("canopen config servo thread start failed!\n");
	}
	else
	{
		rt_thread_startup(tid);
	}
}

// 主站预处理线程
void canopen_start_thread_entry(void *parameter)
{

	// thread delay
	rt_thread_delay(200);
	MConfigServo(SERVO_1); //	配置伺服驱动1
	rt_thread_delay(200);
	MConfigServo(SERVO_2); //	配置伺服驱动2
	rt_thread_delay(200);
	MConfigServo(SERVO_3); //	配置伺服驱动3

	//	masterSendNMTnodeguard() 启动 slave自启线程
	eDriver.od_Data->post_SlaveBootup = slaveBootupHdl;	

	// 设置主站进入操作状态
	setState(eDriver.od_Data, Operational);  

	// 重置从站并设置三个从站进入操作状态
    //masterSendNMTstateChange(eDriver.od_Data, ALL_SERVO_NODE, NMT_Reset_Node);
	rt_thread_delay(100);
    masterSendNMTstateChange(eDriver.od_Data, ALL_SERVO_NODE, NMT_Start_Node);
	
	// 同步发生器(10ms)  
	// 如果使用SDO方式发送需要关闭该项功能 
    MSetSYNC(SYNC_TIME);
	
	edriver_boot_servo(SERVO_1);
	edriver_boot_servo(SERVO_2);
	edriver_boot_servo(SERVO_3);

}


//从站单次自启线程
static void MConfig_Single_Servo(void *parameter)
{
    uint32_t nodeId;
	nodeId = (uint32_t)parameter;
	MConfigServo(nodeId);
	masterSendNMTstateChange(eDriver.od_Data, nodeId, NMT_Start_Node);
}

// 配置从站
static void MConfigServo(uint8_t nodeId)
{
	do
	{
		if(nodeId <= 1) 
		{
			ERROR_DEBUG("MConfigServo: nodeid == 1 \n");
			break;
		}
		eDriver.semTable[nodeId - 2].case_cnt = 0;
		eDriver.semTable[nodeId - 2].retry_cnt = 0;
		eDriver.semTable[nodeId - 2].nodeId = nodeId;
		rt_sem_init(&(eDriver.semTable[nodeId - 2].finish_sem), "servocnf", 0, RT_IPC_FLAG_FIFO);

		//	只执行一次该函数，然后释放锁-等待回调函数的递归-释放信号量
		//	在锁的权限下依次配置
		EnterMutex();   // 进入排斥区
		MConfig_Servo_Param(nodeId, &eDriver.semTable[nodeId - 2]);     // 配置伺服从站
		LeaveMutex();   // 离开排斥区
		rt_sem_take(&(eDriver.semTable[nodeId - 2].finish_sem), RT_WAITING_FOREVER);
		rt_sem_detach(&(eDriver.semTable[nodeId - 2].finish_sem));
	} while (0);
}



static void MConfig_Servo_Param_Callback(CO_Data* d, UNS8 nodeId)
{
    UNS32 abortCode;
	UNS8 res;

    servo_sem_t* conf = RT_NULL;

    conf = &eDriver.semTable[nodeId - 2];

	res = getWriteResultNetworkDict(eDriver.od_Data, nodeId, &abortCode);
	closeSDOtransfer(eDriver.od_Data, nodeId, SDO_CLIENT);
	if(res != SDO_FINISHED)
	{
		conf->retry_cnt++;
		ERROR_DEBUG("MConfig_Servo_Param_Callback(): write SDO failed!  nodeId = %d, abortCode = 0x%08X\n", nodeId, abortCode);
		if(conf->retry_cnt < 3)
		{
			MConfig_Servo_Param(nodeId, conf);
		}
		else
		{
			rt_sem_release(&(conf->finish_sem));
			conf->case_cnt = 0;
			conf->retry_cnt = 0;
			conf->nodeId = 0;
			ERROR_DEBUG("MConfig_Servo_Param_Callback() : SDO config try count > 3, servo config failed!\n");
		}
	}
	else
	{
		conf->case_cnt++;
		conf->retry_cnt = 0;
		conf->nodeId = nodeId;
		MConfig_Servo_Param(nodeId, conf);
	}
}

static void MConfig_Servo_Param(uint8_t nodeId, servo_sem_t* conf)
{
	switch(nodeId)
	{
		case	SERVO_1: 
		//	edriver_clear_error(SERVO_1);
			(void)MServoConfig_Fir(conf);
			break;
		case	SERVO_2:
			//edriver_clear_error(SERVO_2);
			(void)MServoConfig_Fir(conf);
			break;
		case	SERVO_3:
			//edriver_clear_error(SERVO_3);
			(void)MServoConfig_Fir(conf);
			break;

		default: break;	//ignore
	}
}

///////////////////////
//	配置每台机器的第一个PDO	（该接口可以使用）
static int MServoConfig_Fir(servo_sem_t* conf)
{
	switch(conf->case_cnt)
	{
		 case 0:
		 	{	//	清除错误
		 		rt_uint16_t val = 0x80;
		 		writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x6040, 0, 2, uint16, &val, MConfig_Servo_Param_Callback, 0);
		 	}
 		 	break;
		case 1:
			{	//	关闭同步头
				rt_uint32_t shutdown_SYNC = 0x80;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1005, 0, 4, uint32, &shutdown_SYNC, MConfig_Servo_Param_Callback, 0);
			}
 			break;
		case 2:
			{	//	开启同步周期  //如果使用SDO方式发送需要关闭该项功能
				rt_uint32_t startSYNCPeriod = SERVO_SYNC_TIME;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1006, 0, 4, uint32, &startSYNCPeriod, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 3:
			{	//	配置从站模式为轮廓位置模式
				modes_of_operation[0] = PROFILE_POSITION_MODE;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x6060, 0, 1, uint8, &modes_of_operation[0], MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 4:
			{	//	目标位置归零
				rt_int32_t target_position_send = 0;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x607A, 0, 4, int32, &target_position_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 5:
			{	//	初始速度 12,384
				rt_uint32_t velocity_send = 0x3060;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x6081, 0, 4, uint32, &velocity_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 6:
			{	//	初始加速度 12384
				rt_uint32_t acceleration_send = 0x3060;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x6083, 0, 4, uint32, &acceleration_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 7:
			{	//	初始减速度 12384
				rt_uint32_t deceleration_send = 0x3060;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x6084, 0, 4, uint32, &deceleration_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
//-------------------------------------从站RPDO的配置
		case 8:
			{ 	//	关闭RPDO权限
				rt_uint32_t RPDO_COB_ID_shut = 0x80000200 + conf->nodeId;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1400, 1, 4, uint32, &RPDO_COB_ID_shut, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 9:
			{	//	同步模式 ：同步帧 (1帧)
				rt_uint8_t R_transfer_Type = TPDO_TRANSMISSION_SYNC(1);
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1400, 2, 1, uint8, &R_transfer_Type, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 10:
			{	//	关闭RPDO的映射权限
				rt_uint8_t RPDO_MAP_CNT_shut = 0;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1600, 0, 1, uint8, &RPDO_MAP_CNT_shut, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 11:
			{	//	控制字
				rt_uint32_t controlword_map = 0x60400010;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1600, 1, 4, uint32, &controlword_map, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 12:
			{	//	目标位置
				rt_uint32_t target_position_send_map = 0x607A0020;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1600, 2, 4, uint32, &target_position_send_map, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 13:
			{ //  打开RPDO的映射权限
				rt_uint8_t RPDO_MAP_CNT_open = 2;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1600, 0, 1, uint8, &RPDO_MAP_CNT_open, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 14:
			{ // 打开RPDO_COBID权限
				rt_uint32_t RPDO_COB_ID_open = 0x00000200 + conf->nodeId;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1400, 1, 4, uint32, &RPDO_COB_ID_open, MConfig_Servo_Param_Callback, 0);
			}
			break;
//-------------------------------------从站TPDO的配置
		case 15:
			{	// TPDO权限关闭
				rt_uint32_t TPDO_COBID_shut = 0x80000180 + conf->nodeId;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1800, 1, 4, uint32, &TPDO_COBID_shut, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 16:
			{	//	同步帧	1帧
				rt_uint8_t TtransferType = TPDO_TRANSMISSION_SYNC(1);
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1800, 2, 1, uint8, &TtransferType, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 17:
			{	//	设置抑制时间
				rt_uint16_t inhibit_time = 0x64;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1800, 3, 2, uint16, &inhibit_time, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 18:
			{	//	事件定时器
				rt_uint16_t event_timer = 0;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1800, 5, 2, uint16, &event_timer, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 19:
			{ 	//	禁止TPDO的映射权限
				rt_uint8_t highestSubIndex_shut = 0;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A00, 0, 1, uint8, &highestSubIndex_shut, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 20:
			{ 	// 实际位置
				rt_uint32_t actualPositon_send = 0x60640020;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A00, 1, 4, uint32, &actualPositon_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 21:
			{	//  状态字
				rt_uint32_t status_word_send = 0x60410010;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A00, 2, 4, uint32, &status_word_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 22:
			{	//  模式显示
				rt_uint32_t  display_mode_send = 0x60610008;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A00, 3, 4, uint32, &display_mode_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 23:
			{	//	打开TDDO的映射权限
				rt_uint8_t highestSubIndex_open = 3;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A00, 0, 1, uint8, &highestSubIndex_open, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 24:
			{	// 打开TPDO的权限
				rt_uint32_t TPDO_COBID_open = 0x00000180 + conf->nodeId;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1800, 1, 4, uint32, &TPDO_COBID_open, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 25:
			{	//	开启同步头
				rt_uint32_t open_sync_header = 0x40000080;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1005, 0, 4, uint32, &open_sync_header, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 26:
			{	//释放信号量 结束循环
				rt_sem_release(&(conf->finish_sem));
			}
			break;
		default:
			break;
	}
	return 0;
}

//	配置每台机器第二个PDO （该接口不可使用、未分配静态存储区提供给数据进行存储）
static int MServoConfig_Sec(servo_sem_t* conf)
{
	switch(conf->case_cnt)
	{
		case 0:
			{	//	关闭同步头
				rt_uint32_t shutdown_SYNC = 0x80;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1005, 0, 4, uint32, &shutdown_SYNC, MConfig_Servo_Param_Callback, 0);
			}
 			break;
		case 1:
			{	//	开启同步周期  //如果使用SDO方式发送需要关闭该项功能
				rt_uint32_t startSYNCPeriod = SERVO_SYNC_TIME;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1006, 0, 4, uint32, &startSYNCPeriod, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 2:
			{	//	配置从站模式为轮廓位置模式
				modes_of_operation[0] = PROFILE_POSITION_MODE;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x6060, 0, 1, uint8, &modes_of_operation[0], MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 3:
			{	//	目标位置归零
				rt_int32_t target_position_send = 0;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x607A, 0, 4, int32, &target_position_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 4:
			{	//	初始速度 12,384
				rt_uint32_t velocity_send = 0x3060;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x6081, 0, 4, uint32, &velocity_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 5:
			{	//	初始加速度 12384
				rt_uint32_t acceleration_send = 0x3060;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x6083, 0, 4, uint32, &acceleration_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 6:
			{	//	初始减速度 12384
				rt_uint32_t deceleration_send = 0x3060;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x6084, 0, 4, uint32, &deceleration_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
//-------------------------------------从站RPDO的配置
		case 7:
			{ 	//	关闭RPDO权限
				rt_uint32_t RPDO_COB_ID_shut = 0x80000300 + conf->nodeId;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1401, 1, 4, uint32, &RPDO_COB_ID_shut, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 8:
			{	//	同步模式 ：同步帧 (1帧)
				rt_uint8_t R_transfer_Type = TPDO_TRANSMISSION_SYNC(1);
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1401, 2, 1, uint8, &R_transfer_Type, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 9:
			{	//	关闭RPDO的映射权限
				rt_uint8_t RPDO_MAP_CNT_shut = 0;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1601, 0, 1, uint8, &RPDO_MAP_CNT_shut, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 10:
			{	//	控制字
				rt_uint32_t controlword_map = 0x60400010;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1601, 1, 4, uint32, &controlword_map, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 11:
			{	//	目标位置
				rt_uint32_t target_position_send_map = 0x607A0020;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1601, 2, 4, uint32, &target_position_send_map, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 12:
			{ //  打开RPDO的映射权限
				rt_uint8_t RPDO_MAP_CNT_open = 2;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1601, 0, 1, uint8, &RPDO_MAP_CNT_open, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 13:
			{ // 打开RPDO_COBID权限
				rt_uint32_t RPDO_COB_ID_open = 0x00000300 + conf->nodeId;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1401, 1, 4, uint32, &RPDO_COB_ID_open, MConfig_Servo_Param_Callback, 0);
			}
			break;
//-------------------------------------从站TPDO的配置
		case 14:
			{	// TPDO权限关闭
				rt_uint32_t TPDO_COBID_shut = 0x80000280 + conf->nodeId;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1801, 1, 4, uint32, &TPDO_COBID_shut, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 15:
			{	//	同步帧	1帧
				rt_uint8_t TtransferType = TPDO_TRANSMISSION_SYNC(1);
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1801, 2, 1, uint8, &TtransferType, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 16:
			{	//	设置抑制时间
				rt_uint16_t inhibit_time = 0x64;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1801, 3, 2, uint16, &inhibit_time, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 17:
			{	//	事件定时器
				rt_uint16_t event_timer = 0;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1801, 5, 2, uint16, &event_timer, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 18:
			{ 	//	禁止TPDO的映射权限
				rt_uint8_t highestSubIndex_shut = 0;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A01, 0, 1, uint8, &highestSubIndex_shut, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 19:
			{ 	// 实际位置
				rt_uint32_t actualPositon_send = 0x60640020;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A01, 1, 4, uint32, &actualPositon_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 20:
			{	//  状态字
				rt_uint32_t status_word_send = 0x60410010;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A01, 2, 4, uint32, &status_word_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 21:
			{	//  模式显示
				rt_uint32_t  display_mode_send = 0x60610008;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A01, 3, 4, uint32, &display_mode_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 22:
			{	//	打开TDDO的映射权限
				rt_uint8_t highestSubIndex_open = 3;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A01, 0, 1, uint8, &highestSubIndex_open, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 23:
			{	// 打开TPDO的权限
				rt_uint32_t TPDO_COBID_open = 0x00000280 + conf->nodeId;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1801, 1, 4, uint32, &TPDO_COBID_open, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 24:
			{	//	开启同步头
				rt_uint32_t open_sync_header = 0x40000080;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1005, 0, 4, uint32, &open_sync_header, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 25:
			{	//释放信号量 结束循环
				rt_sem_release(&(conf->finish_sem));
			}
			break;
		default:
			break;
	}
	return 0;
}

//	配置每台机器第三个PDO （该接口不可使用、未分配静态存储区 提供给数据进行存储）
static int MServoConfig_Trd(servo_sem_t* conf)
{
	switch(conf->case_cnt)
	{
		case 0:
			{	//	关闭同步头
				rt_uint32_t shutdown_SYNC = 0x80;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1005, 0, 4, uint32, &shutdown_SYNC, MConfig_Servo_Param_Callback, 0);
			}
 			break;
		case 1:
			{	//	开启同步周期  //如果使用SDO方式发送需要关闭该项功能
				rt_uint32_t startSYNCPeriod = SERVO_SYNC_TIME;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1006, 0, 4, uint32, &startSYNCPeriod, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 2:
			{	//	配置从站模式为轮廓位置模式
				modes_of_operation[0] = PROFILE_POSITION_MODE;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x6060, 0, 1, uint8, &modes_of_operation[0], MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 3:
			{	//	目标位置归零
				rt_int32_t target_position_send = 0;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x607A, 0, 4, int32, &target_position_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 4:
			{	//	初始速度 12,384
				rt_uint32_t velocity_send = 0x3060;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x6081, 0, 4, uint32, &velocity_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 5:
			{	//	初始加速度 12384
				rt_uint32_t acceleration_send = 0x3060;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x6083, 0, 4, uint32, &acceleration_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 6:
			{	//	初始减速度 12384
				rt_uint32_t deceleration_send = 0x3060;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x6084, 0, 4, uint32, &deceleration_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
//-------------------------------------从站RPDO的配置
		case 7:
			{ 	//	关闭RPDO权限
				rt_uint32_t RPDO_COB_ID_shut = 0x80000400 + conf->nodeId;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1402, 1, 4, uint32, &RPDO_COB_ID_shut, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 8:
			{	//	同步模式 ：同步帧 (1帧)
				rt_uint8_t R_transfer_Type = TPDO_TRANSMISSION_SYNC(1);
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1402, 2, 1, uint8, &R_transfer_Type, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 9:
			{	//	关闭RPDO的映射权限
				rt_uint8_t RPDO_MAP_CNT_shut = 0;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1602, 0, 1, uint8, &RPDO_MAP_CNT_shut, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 10:
			{	//	控制字
				rt_uint32_t controlword_map = 0x60400010;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1602, 1, 4, uint32, &controlword_map, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 11:
			{	//	目标位置
				rt_uint32_t target_position_send_map = 0x607A0020;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1602, 2, 4, uint32, &target_position_send_map, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 12:
			{ //  打开RPDO的映射权限
				rt_uint8_t RPDO_MAP_CNT_open = 2;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1602, 0, 1, uint8, &RPDO_MAP_CNT_open, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 13:
			{ // 打开RPDO_COBID权限
				rt_uint32_t RPDO_COB_ID_open = 0x00000400 + conf->nodeId;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1402, 1, 4, uint32, &RPDO_COB_ID_open, MConfig_Servo_Param_Callback, 0);
			}
			break;
//-------------------------------------从站TPDO的配置
		case 14:
			{	// TPDO权限关闭
				rt_uint32_t TPDO_COBID_shut = 0x80000380 + conf->nodeId;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1802, 1, 4, uint32, &TPDO_COBID_shut, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 15:
			{	//	同步帧	1帧
				rt_uint8_t TtransferType = TPDO_TRANSMISSION_SYNC(1);
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1802, 2, 1, uint8, &TtransferType, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 16:
			{	//	设置抑制时间
				rt_uint16_t inhibit_time = 0x64;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1802, 3, 2, uint16, &inhibit_time, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 17:
			{	//	事件定时器
				rt_uint16_t event_timer = 0;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1802, 5, 2, uint16, &event_timer, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 18:
			{ 	//	禁止TPDO的映射权限
				rt_uint8_t highestSubIndex_shut = 0;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A02, 0, 1, uint8, &highestSubIndex_shut, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 19:
			{ 	// 实际位置
				rt_uint32_t actualPositon_send = 0x60640020;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A02, 1, 4, uint32, &actualPositon_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 20:
			{	//  状态字
				rt_uint32_t status_word_send = 0x60410010;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A02, 2, 4, uint32, &status_word_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 21:
			{	//  模式显示
				rt_uint32_t  display_mode_send = 0x60610008;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A02, 3, 4, uint32, &display_mode_send, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 22:
			{	//	打开TDDO的映射权限
				rt_uint8_t highestSubIndex_open = 3;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1A02, 0, 1, uint8, &highestSubIndex_open, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 23:
			{	// 打开TPDO的权限
				rt_uint32_t TPDO_COBID_open = 0x00000380 + conf->nodeId;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1802, 1, 4, uint32, &TPDO_COBID_open, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 24:
			{	//	开启同步头
				rt_uint32_t open_sync_header = 0x40000080;
				writeNetworkDictCallBack(eDriver.od_Data, conf->nodeId, 0x1005, 0, 4, uint32, &open_sync_header, MConfig_Servo_Param_Callback, 0);
			}
			break;
		case 25:
			{	//释放信号量 结束循环
				rt_sem_release(&(conf->finish_sem));
			}
			break;
		default:
			break;
	}
	return 0;
}

// 设置心跳机制
void MSetHeartBeat(void)
{
    UNS32 size;
	UNS32 consumer_heartbeat_time;

    // ConsumerHeartbeatEntries[1] 132072
	consumer_heartbeat_time = (2 << 16) | CONSUMER_HEARTBEAT_TIME;  
	size = 4;
	writeLocalDict(eDriver.od_Data, 0x1016, 1, &consumer_heartbeat_time, &size, 0); // 节点2

	// ConsumerHeartbeatEntries[2] 197,608
	consumer_heartbeat_time = (3 << 16) | CONSUMER_HEARTBEAT_TIME;
	size = 4;
	writeLocalDict(eDriver.od_Data, 0x1016, 2, &consumer_heartbeat_time, &size, 0);	// 节点3

	// ConsumerHeartbeatEntries[3] 
	consumer_heartbeat_time = (4 << 16) | CONSUMER_HEARTBEAT_TIME;
	size = 4;
	writeLocalDict(eDriver.od_Data, 0x1016, 3, &consumer_heartbeat_time, &size, 0);	// 节点4
}


//////////////////////
//	激活节点同步发生器
static void MSetSYNC(rt_uint32_t cycle)
{
    UNS8 data_type;
	UNS32 sync_id, size, period;

	data_type = uint32;
	size = 4;
	readLocalDict(eDriver.od_Data, 0x1005, 0, &sync_id, &size, &data_type, 0);

    // 1<<30 激活次高位设定 0x4000 0000
	sync_id |= (1 << 30);
	writeLocalDict(eDriver.od_Data, 0x1005, 0, &sync_id, &size, 0);

    // 同步通信周期字典设定
    period = (UNS32)cycle;
	writeLocalDict(eDriver.od_Data, 0x1006, 0, &period, &size, 0);
}

// 写入网络字典后的回调函数（通用版）
static void write_net_dic_callback(CO_Data* d, UNS8 nodeId)
{
	UNS32 abortCode;
	UNS8 res;
	res = getWriteResultNetworkDict(eDriver.od_Data, nodeId, &abortCode);
	closeSDOtransfer(eDriver.od_Data, nodeId, SDO_CLIENT);
	if(res != SDO_FINISHED)
        ERROR_DEBUG(" writeStatus_Callback() fail! \n");
}



// 设置控制字
void edriver_set_control_word(rt_uint8_t nodeid, rt_uint16_t val)
{
	switch (nodeid)
	{
	case SERVO_1:
    	Controlword[0] = (UNS16)val;
		break;
	case SERVO_2:
    	Controlword[1] = (UNS16)val;
		break;
	case SERVO_3:
    	Controlword[2] = (UNS16)val;
		break;

	default:	//忽略
		break;
	}
    EDRIVER_MDELAY(WORD_WAIT_TIME);
}

// 	引导伺服驱动器进入使能状态
//	初始化则设置位置归零
void edriver_boot_servo(rt_uint8_t nodeid)
{

	switch (nodeid)
	{
	case SERVO_1:
		{
			//	伺服上电后 8s 完成初始化，请在初始化完成后再控制伺服
			//	只在第一个从站等待8s，假定三台设备是在同一时间进行上电，不然就将此行代码放到接口开始处
			//	EDRIVER_MDELAY(8000);

			Controlword[0] = 0x06;
			EDRIVER_MDELAY(WORD_WAIT_TIME);
			Controlword[0] = 0x07;
			EDRIVER_MDELAY(WORD_WAIT_TIME);
			Controlword[0] = 0x0F;
			EDRIVER_MDELAY(WORD_WAIT_TIME);
			//edriver_active_target(SERVO_1);			
			//EDRIVER_MDELAY(WORD_WAIT_TIME);			
		}
		break;
	case SERVO_2:
		{
			Controlword[1] = 0x06;
			EDRIVER_MDELAY(WORD_WAIT_TIME);
			Controlword[1] = 0x07;
			EDRIVER_MDELAY(WORD_WAIT_TIME);
			Controlword[1] = 0x0F;
			EDRIVER_MDELAY(WORD_WAIT_TIME);
			//edriver_active_target(SERVO_2);			
			//EDRIVER_MDELAY(WORD_WAIT_TIME);
		}
		break;
	case SERVO_3:
		{
			Controlword[2] = 0x06;
			EDRIVER_MDELAY(WORD_WAIT_TIME);
			Controlword[2] = 0x07;
			EDRIVER_MDELAY(WORD_WAIT_TIME);
			Controlword[2] = 0x0F;
			EDRIVER_MDELAY(WORD_WAIT_TIME);
			//edriver_active_target(SERVO_3);			
			//EDRIVER_MDELAY(WORD_WAIT_TIME);
		}
		break;

	default:
		break;
	}
}

// 激活目标更新位置
void edriver_active_target(rt_uint8_t nodeid)
{
	if(nodeid == SERVO_1)
	{
    	Controlword[0] = ENABLE_UPDATE_TO_TAR_POS;
	}
	else if(nodeid == SERVO_2)
	{
		Controlword[1] = ENABLE_UPDATE_TO_TAR_POS;
	}
	else
	{
		Controlword[2] = ENABLE_UPDATE_TO_TAR_POS;
	}
}


///////////////////////////////////////////////////////////////////////
// 从站执行完毕后的回调函数输出，或者通信失败提示
static void readStatus_Callback(CO_Data* d, UNS8 nodeId)
{
	UNS32 abortCode;
	UNS8 res;
    UNS16 status;
    UNS32  size;
	res = getReadResultNetworkDict(eDriver.od_Data, nodeId, (void*)&status, &size, &abortCode);
	status_test = status;
	closeSDOtransfer(eDriver.od_Data, nodeId, SDO_CLIENT);
	if(res != SDO_FINISHED)
	{
        ERROR_DEBUG(" Get statusword fail! \n");
	}
	else
	{
	    INFO_DEBUG(" servo statusword %x interpolationmode:%d == 1? --- 1 is interpolaition mode \n", status, (status&0x800)==0x800);
    }

}

//获取当前位置
static void readPos_Callback(CO_Data* d, UNS8 nodeId)
{
	UNS32 abortCode;
	UNS8 res;
	UNS32	data;
    UNS32  size;
	res = getReadResultNetworkDict(eDriver.od_Data, nodeId, (void*)&data, &size, &abortCode);
	//position_test = data;
	closeSDOtransfer(eDriver.od_Data, nodeId, SDO_CLIENT);
	if(res != SDO_FINISHED) ERROR_DEBUG(" readPos_Callback() fail!\n");
}


// 获取三个从站的状态字信息
int MGetServoStatus()
{
    UNS8  size;
    size = readNetworkDictCallback (eDriver.od_Data, SERVO_1, 0x6041, 0, uint16, readStatus_Callback, 0);
    if(size != 0)
	{
        ERROR_DEBUG(" Get statusword fail!\n");
    }
    size = readNetworkDictCallback (eDriver.od_Data, SERVO_2, 0x6041, 0, uint16, readStatus_Callback, 0);
    if(size != 0)
	{
        ERROR_DEBUG(" Get statusword fail!\n");
    }

    size = readNetworkDictCallback (eDriver.od_Data, SERVO_3, 0x6041, 0, uint16, readStatus_Callback, 0);
    if(size != 0)
	{
        ERROR_DEBUG(" Get statusword fail!\n");
    }
    return (int)size;
}

// 获取从站状态字
static void writeStatus_Callback(CO_Data* d, UNS8 nodeId)
{
	UNS32 abortCode;
	UNS8 res;
	res = getWriteResultNetworkDict(eDriver.od_Data, nodeId, &abortCode);
	closeSDOtransfer(eDriver.od_Data, nodeId, SDO_CLIENT);
	if(res != SDO_FINISHED)
        ERROR_DEBUG(" writeStatus_Callback() fail! \n");

}

// 获取写入从站的60C1 位置
static void write60C1_Callback(CO_Data* d, UNS8 nodeId)
{
	UNS32 abortCode;
	UNS8 res;
	res = getWriteResultNetworkDict(eDriver.od_Data, nodeId,&abortCode);
	closeSDOtransfer(eDriver.od_Data, nodeId, SDO_CLIENT);
	if(res != SDO_FINISHED)
    	ERROR_DEBUG(" write60C1_Callback fail! \n");


}
///////////////////////////////////////////////////////////////////////

// SDO方式从站引导流程
int MSetEDriverControl(rt_uint8_t nodeid)
{

	UNS8 size;
	rt_uint16_t	control_instruction;

	//进入引导流程
	// UNS8 mode_open = PROFILE_POSITION_MODE;
	// writeNetworkDictCallBack(eDriver.od_Data, nodeid, 0x6060, 0, 1, uint8, &mode_open, writeStatus_Callback, 0);
	// EDRIVER_MDELAY(WORD_WAIT_TIME);
	
	control_instruction = 0x6;
	writeNetworkDictCallBack(eDriver.od_Data, nodeid, 0x6040, 1, 2, uint16, (void*)&control_instruction, writeStatus_Callback, 0);
	EDRIVER_MDELAY(WORD_WAIT_TIME);
	
	control_instruction = 0x7;
	writeNetworkDictCallBack(eDriver.od_Data, nodeid, 0x6040, 1, 2, uint16, (void*)&control_instruction, writeStatus_Callback, 0);
	EDRIVER_MDELAY(WORD_WAIT_TIME);
	
	control_instruction = 0xF;
	writeNetworkDictCallBack(eDriver.od_Data, nodeid, 0x6040, 1, 2, uint16, (void*)&control_instruction, writeStatus_Callback, 0);
	EDRIVER_MDELAY(WORD_WAIT_TIME);
	
	control_instruction = 0x1F;
	writeNetworkDictCallBack(eDriver.od_Data, nodeid, 0x6040, 1, 2, uint16, (void*)&control_instruction, writeStatus_Callback, 0);
	EDRIVER_MDELAY(WORD_WAIT_TIME);

	//读取状态字
	size = readNetworkDictCallback (eDriver.od_Data, nodeid, 0x6041, 0, uint16, readStatus_Callback, 0);
	if(size != 0){
		ERROR_DEBUG(" Get statusword fail!\n");
	}

	// 激活插补模式
	// control_instruction = 0x1F;
	// writeNetworkDictCallBack(eDriver.od_Data, nodeid, 0x6040, 0, 2, uint16, (void*)&control_instruction, writeStatus_Callback, 0);
	// EDRIVER_MDELAY(WORD_WAIT_TIME);

	// size = readNetworkDictCallback (eDriver.od_Data, nodeid, 0x6064, 0, int32, readPos_Callback, 0);
	// if(size != 0)
	// {
	// 	ERROR_DEBUG(" Get statusword fail!");
	// }


	// size = readNetworkDictCallback (eDriver.od_Data, nodeid, 0x6064, 0, int32, readPos_Callback, 0);
	// if(size != 0)
	// {
	// 	ERROR_DEBUG(" Get readNetworkDictCallback 0x6064 fail!\n");
	// }

	//	读取状态字和当前位置
	INFO_DEBUG("  servo statusword %x  Servo id:%d		\n", 		status_test 	, nodeid);
	//INFO_DEBUG("  servo position_test %x  Servo id:%d	\n", 		position_test 	, nodeid);


	return 0;
}

int MSDOSetPosition(rt_uint8_t nodeid, rt_uint32_t *position)
{
	rt_uint16_t pos1, pos2;
	UNS16 interpolation;
	
	// 入参检查
	if(!position)
	{
		ERROR_DEBUG(" position == NULL !\n");
		return 0;
	}

	pos1 = (rt_uint16_t)(*position & 0xFFFF);
	pos2 = (rt_uint16_t)((*position >> 16) & 0xFFFF);
	
	interpolation = 0x1F;
	writeNetworkDictCallBack(eDriver.od_Data, nodeid, 0x6040, 0, 2, uint16, &interpolation, writeStatus_Callback, 0);

	// 写入从机需要改变的位置节点
	if(nodeid == SERVO_1){
		writeNetworkDictCallBack(eDriver.od_Data, nodeid, 0x60C1, 1, 2, uint16, (void*)&pos1, write60C1_Callback, 0);
		writeNetworkDictCallBack(eDriver.od_Data, nodeid, 0x60C1, 2, 2, uint16, (void*)&pos2, write60C1_Callback, 0);
	}else if(nodeid == SERVO_2 ){
		writeNetworkDictCallBack(eDriver.od_Data, nodeid, 0x60C1, 1, 2, uint16, (void*)&pos1, write60C1_Callback, 0);
		writeNetworkDictCallBack(eDriver.od_Data, nodeid, 0x60C1, 2, 2, uint16, (void*)&pos2, write60C1_Callback, 0);
	}else if(nodeid == SERVO_3 ){
		writeNetworkDictCallBack(eDriver.od_Data, nodeid, 0x60C1, 1, 2, uint16, (void*)&pos1, write60C1_Callback, 0);
		writeNetworkDictCallBack(eDriver.od_Data, nodeid, 0x60C1, 2, 2, uint16, (void*)&pos2, write60C1_Callback, 0);
	}else{
		ERROR_DEBUG(" unknowwn nodeid :  %d! \n",nodeid);
	}
	return 0;
}

int MInterpolationSetPosition(rt_uint8_t	nodeID, rt_uint32_t *position)
{
	rt_uint16_t pos1, pos2;
	// 入参检查
	if(!position)
	{
		ERROR_DEBUG(" MInterpolationSetPosition() : position is NULL !\n");
		return -1;
	}

	pos1 = (rt_uint16_t)(*position & 0xFFFF);
	pos2 = (rt_uint16_t)((*position >> 16) & 0xFFFF);

	if(nodeID == SERVO_1){
		Interpolation_value[0] = (UNS16)pos1;
		Interpolation_value[1] = (UNS16)pos2;
		INFO_DEBUG(" servo 1 Interpolation_value :  低位%d--高位%d \n",	Interpolation_value[0],Interpolation_value[1]);

	}else if(nodeID == SERVO_2){
		Interpolation_value[2] = (UNS16)pos1;
		Interpolation_value[3] = (UNS16)pos2;
		INFO_DEBUG(" servo 2 Interpolation_value :  低位%d--高位%d \n",	Interpolation_value[2],Interpolation_value[3]);

	}else if(nodeID == SERVO_3){		
		Interpolation_value[4] = (UNS16)pos1;
		Interpolation_value[5] = (UNS16)pos2;
		INFO_DEBUG(" servo 3 Interpolation_value :  低位%d--高位%d \n",	Interpolation_value[4],Interpolation_value[5]);

	}else{
		return -1;
	}
	
	return 0;
}


rt_uint32_t edriver_brake_and_release_the_brake(rt_uint8_t  nodeid, rt_bool_t  open_or_release)
{
	rt_uint32_t	val;
	
	if(open_or_release == OPEN_BRAKE)
	{
		val = 0;
		//	启动刹车
		switch (nodeid)
		{
		case SERVO_1:
			writeNetworkDictCallBack(eDriver.od_Data, SERVO_1, 0x4602, 0, 4, uint32, &val, write_net_dic_callback, 0);
			break;
		case SERVO_2:
			writeNetworkDictCallBack(eDriver.od_Data, SERVO_2, 0x4602, 0, 4, uint32, &val, write_net_dic_callback, 0);
			break;
		case SERVO_3:
			writeNetworkDictCallBack(eDriver.od_Data, SERVO_3, 0x4602, 0, 4, uint32, &val, write_net_dic_callback, 0);
			break;
		default:
			break;
		}
	}
	else if(open_or_release == RELEASE_BRAKE)
	{
		val = 1;
		//	释放刹车
		switch (nodeid)
		{
		case SERVO_1:
			writeNetworkDictCallBack(eDriver.od_Data, SERVO_1, 0x4602, 0, 4, uint32, &val, write_net_dic_callback, 0);
			break;
		case SERVO_2:
			writeNetworkDictCallBack(eDriver.od_Data, SERVO_2, 0x4602, 0, 4, uint32, &val, write_net_dic_callback, 0);
			break;
		case SERVO_3:
			writeNetworkDictCallBack(eDriver.od_Data, SERVO_3, 0x4602, 0, 4, uint32, &val, write_net_dic_callback, 0);
			break;
		default:
			break;
		}

	}
	else
	{
		return ERROR_BRAKE;
	}

	return 0;
}

//	查看错误和清除错误的回调函数
static void servo_read_error_callback(CO_Data* d, UNS8 nodeId)
{
	UNS32 abortCode;
	UNS8 res;
    UNS16 error;
    UNS32  size;
	res = getReadResultNetworkDict(eDriver.od_Data, nodeId, (void*)&error, &size, &abortCode);

	closeSDOtransfer(eDriver.od_Data, nodeId, SDO_CLIENT);
	if(res != SDO_FINISHED)
	{
        ERROR_DEBUG(" read_error_callback() fail! \n");
	}
	else
	{
	    INFO_DEBUG(" servo read_error_callback(): %d \n", error);
    }
}

//	清除错误
rt_uint32_t edriver_clear_error(rt_uint8_t  nodeid)
{
	rt_uint8_t	rc = 0;
	rt_uint16_t	val = 0x80;
	switch (nodeid)
	{
	case SERVO_1:
		{
			rc = readNetworkDictCallback(eDriver.od_Data, SERVO_1, 0x603F, 0, uint16, servo_read_error_callback, 0);
			if(rc != 0)
			{
				ERROR_DEBUG(" edriver_clear_error() fail!\n");
			}
			writeNetworkDictCallBack(eDriver.od_Data, SERVO_1, 0x6040, 0, 2, uint16, &val, write_net_dic_callback, 0);
		}
		break;
	case SERVO_2:
		{
			rc = readNetworkDictCallback(eDriver.od_Data, SERVO_2, 0x603F, 0, uint16, servo_read_error_callback, 0);
			if(rc != 0)
			{
				ERROR_DEBUG(" edriver_clear_error() fail!\n");
			}
			writeNetworkDictCallBack(eDriver.od_Data, SERVO_2, 0x6040, 0, 2, uint16, &val, write_net_dic_callback, 0);
		}
		break;
	case SERVO_3:
		{
			rc = readNetworkDictCallback(eDriver.od_Data, SERVO_3, 0x603F, 0, uint16, servo_read_error_callback, 0);
			if(rc != 0)
			{
				ERROR_DEBUG(" edriver_clear_error() fail!\n");
			}
			writeNetworkDictCallBack(eDriver.od_Data, SERVO_3, 0x6040, 0, 2, uint16, &val, write_net_dic_callback, 0);
		}
		break;
	default:	// 忽略
		break;
	}

	return (rt_uint32_t)rc;
}

//	配置零差云控的节点ID
// void edriver_config_servo_canid()
// {
// 	rt_uint32_t	val = 0x65766173;
// 	writeNetworkDictCallBack(eDriver.od_Data, 1, 0x1010, 01, 4, uint32, &val, write_net_dic_callback, 0);
// }

//	实际位置、状态字、模式显示的打印
void edriver_print_60XX_info()
{
	INFO_DEBUG("servo_1 : actual position:%x | mode display:%x | status word %x \n", Position_actual_value[0], Modes_of_operation_display[0], Statusword[0]);
	INFO_DEBUG("servo_2 : actual position:%x | mode display:%x | status word %x \n", Position_actual_value[1], Modes_of_operation_display[1], Statusword[1]);
	INFO_DEBUG("servo_3 : actual position:%x | mode display:%x | status word %x \n", Position_actual_value[2], Modes_of_operation_display[2], Statusword[2]);
}

//	设置操作模式
void MSetOperationMode(rt_uint8_t   nodeid, rt_uint8_t mode)
{
	if(nodeid == SERVO_1)
	{
		modes_of_operation[0] = mode;
		writeNetworkDictCallBack(eDriver.od_Data, SERVO_1, 0x6060, 1, 1, int8, &modes_of_operation[0], write_net_dic_callback, 0);
	}
	else if(nodeid == SERVO_2)
	{
		modes_of_operation[1] = mode;
		writeNetworkDictCallBack(eDriver.od_Data, SERVO_2, 0x6060, 1, 1, int8, &modes_of_operation[1], write_net_dic_callback, 0);
	}
	else if(nodeid == SERVO_3)
	{
		modes_of_operation[2] = mode;
		writeNetworkDictCallBack(eDriver.od_Data, SERVO_3, 0x6060, 1, 1, int8, &modes_of_operation[2], write_net_dic_callback, 0);
	}
	else
	{
		ERROR_DEBUG("MSetOperationMode() failed ! unknown nodeid : %d", nodeid);
	}
	INFO_DEBUG("MSetOperationMode() success set ! the nodeid was : %d | setting mode was: %x ", nodeid, mode);
}

////	函数功能：设置目标位置
//rt_int32_t edriver_set_target_position(rt_uint8_t  nodeid, rt_uint32_t pos)
//{
//	switch (nodeid)
//	{
//	case SERVO_1:
//		target_location[0] = pos;
//		break;
//	case SERVO_2:
//		target_location[1] = pos;
//		break;
//	case SERVO_3:
//		target_location[2] = pos;
//		break;
//	default:
//		break;
//	}
//	return 0;
//}

static void servo_read_actual_pos_callback(CO_Data* d, UNS8 nodeId)
{
	UNS32 abortCode;
	UNS8 res;
    UNS32  size;
	if(nodeId == SERVO_1)
	{
		res = getReadResultNetworkDict(eDriver.od_Data, SERVO_1, (void*)&Position_actual_value[0], &size, &abortCode);	
	}
	else if(nodeId == SERVO_2)
	{
		res = getReadResultNetworkDict(eDriver.od_Data, SERVO_2, (void*)&Position_actual_value[1], &size, &abortCode);
	}
	else
	{
		res = getReadResultNetworkDict(eDriver.od_Data, SERVO_3, (void*)&Position_actual_value[2], &size, &abortCode);
	}

	closeSDOtransfer(eDriver.od_Data, nodeId, SDO_CLIENT);
	if(res != SDO_FINISHED)
	{
        ERROR_DEBUG(" read_error_callback() fail! \n");
	}
	else
	{
	    INFO_DEBUG(" servo read_error_callback(): servo1: %d servo2: %d servo3: %d\n", Position_actual_value[0],Position_actual_value[1],Position_actual_value[2]);
    }
}

//	SDO--设置目标位置(包含了csp模式)
rt_int32_t edriver_sdo_set_tar_position(rt_uint8_t nodeid, rt_int32_t  pos, rt_bool_t csp_mode)
{
	rt_int32_t	rc = 0;
	rt_int32_t	tar_position[3];
	switch (nodeid)
	{
	case SERVO_1:
		if(csp_mode)
		{
			rc = readNetworkDictCallback(eDriver.od_Data, SERVO_1, 0x6064, 0, int32, servo_read_actual_pos_callback, 0);
			if(rc != 0)
			{
				ERROR_DEBUG("edriver_sdo_set_tar_position() -> readNetworkDictCallback()failed nodeid:%s", nodeid);
				return NET_WRI_FAIL;
			}

			tar_position[0] = Position_actual_value[0];
			WriteDicCallBack(eDriver.od_Data, SERVO_1, 0x607A, 0, 4, int32, &tar_position[0], write_net_dic_callback);
		}
		
		tar_position[0] = pos;
		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_1, 0x607A, 0, 4, int32, &tar_position[0], write_net_dic_callback);
		if(rc != 0)
		{
			ERROR_DEBUG("edriver_sdo_set_tar_position() failed nodeid:%s", nodeid);
			return NET_WRI_FAIL;
		}
		break;
	case SERVO_2:
		if(csp_mode)
		{
			rc = readNetworkDictCallback(eDriver.od_Data, SERVO_2, 0x6064, 0, int32, servo_read_actual_pos_callback, 0);
			if(rc != 0)
			{
				ERROR_DEBUG("edriver_sdo_set_tar_position() -> readNetworkDictCallback()failed nodeid:%s", nodeid);
				return NET_WRI_FAIL;
			}

			tar_position[1] = Position_actual_value[1];
			WriteDicCallBack(eDriver.od_Data, SERVO_2, 0x607A, 0, 4, int32, &tar_position[1], write_net_dic_callback);
		}

		tar_position[1] = pos;
		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_2, 0x607A, 0, 4, int32, &tar_position[1], write_net_dic_callback);
		if(rc != 0)
		{
			ERROR_DEBUG("edriver_sdo_set_tar_position() failed nodeid:%s", nodeid);
			return NET_WRI_FAIL;
		}
		break;
	case SERVO_3:
		if(csp_mode)
		{
			rc = readNetworkDictCallback(eDriver.od_Data, SERVO_3, 0x6064, 0, int32, servo_read_actual_pos_callback, 0);
			if(rc != 0)
			{
				ERROR_DEBUG("edriver_sdo_set_tar_position() -> readNetworkDictCallback()failed nodeid:%s", nodeid);
				return NET_WRI_FAIL;
			}

			tar_position[2] = Position_actual_value[2];
			WriteDicCallBack(eDriver.od_Data, SERVO_3, 0x607A, 0, 4, int32, &tar_position[2], write_net_dic_callback);
		}

		tar_position[2] = pos;
		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_3, 0x607A, 0, 4, int32, &tar_position[2], write_net_dic_callback);
		if(rc != 0)
		{
			ERROR_DEBUG("edriver_sdo_set_tar_position() failed nodeid:%s", nodeid);
			return NET_WRI_FAIL;
		}
		break;
	default:
		break;
	}
	return rc;
}


//	函数功能：设置速度
rt_int32_t edriver_set_speed(rt_uint8_t nodeid, rt_uint32_t  speed)
{
	rt_int32_t	rc = 0;
	switch (nodeid)
	{
	case SERVO_1:
		speed_val[0] = speed;
		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_1, 0x6081, 0, 4, uint32, &speed_val[0], write_net_dic_callback);
		if(rc != 0)
		{
			ERROR_DEBUG("edriver_set_speed() failed nodeid:%s", nodeid);
			return NET_WRI_FAIL;
		}
		break;
	case SERVO_2:
		speed_val[1] = speed;
		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_2, 0x6081, 0, 4, uint32, &speed_val[1], write_net_dic_callback);
		if(rc != 0)
		{
			ERROR_DEBUG("edriver_set_speed() failed nodeid:%s", nodeid);
			return NET_WRI_FAIL;
		}
		break;
	case SERVO_3:
		speed_val[2] = speed;
		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_3, 0x6081, 0, 4, uint32, &speed_val[2], write_net_dic_callback);
		if(rc != 0)
		{
			ERROR_DEBUG("edriver_set_speed() failed nodeid:%s", nodeid);
			return NET_WRI_FAIL;
		}
		break;
	default:
		break;
	}
	return rc;
}

////	函数功能：设置目标速度
//rt_int32_t edriver_set_target_speed(rt_uint8_t nodeid, rt_uint32_t  tar_speed)
//{
//	rt_int32_t	rc = 0;

//	switch (nodeid)
//	{
//	case SERVO_1:
//		target_speed_val[0] = tar_speed;
//		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_1, 0x60FF, 0, 4, uint32, &target_speed_val[0], write_net_dic_callback);
//		if(rc != 0)
//		{
//			ERROR_DEBUG("edriver_set_target_speed() failed nodeid:%s", nodeid);
//			return NET_WRI_FAIL;
//		}
//		break;
//	case SERVO_2:
//		target_speed_val[1] = tar_speed;
//		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_2, 0x60FF, 0, 4, uint32, &target_speed_val[1], write_net_dic_callback);
//		if(rc != 0)
//		{
//			ERROR_DEBUG("edriver_set_target_speed() failed nodeid:%s", nodeid);
//			return NET_WRI_FAIL;
//		}
//		break;
//	case SERVO_3:
//		target_speed_val[2] = tar_speed;
//		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_3, 0x60FF, 0, 4, uint32, &target_speed_val[2], write_net_dic_callback);
//		if(rc != 0)
//		{
//			ERROR_DEBUG("edriver_set_target_speed() failed nodeid:%s", nodeid);
//			return NET_WRI_FAIL;
//		}
//		break;
//	default:
//		break;
//	}
//	return rc;
//}

//	函数功能：设置加速度
rt_int32_t edriver_set_acceleration(rt_uint8_t nodeid, rt_uint32_t  acceleration)
{
	rt_int32_t	rc = 0;

	switch (nodeid)
	{
	case SERVO_1:
		acceleration_val[0] = acceleration;
		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_1, 0x6083, 1, 4, uint32, &acceleration_val[0], write_net_dic_callback);
		if(rc != 0)
		{
			ERROR_DEBUG("edriver_set_acceleration() failed nodeid:%s", nodeid);
			return NET_WRI_FAIL;
		}
		break;
	case SERVO_2:
		acceleration_val[1] = acceleration;
		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_2, 0x6083, 1, 4, uint32, &acceleration_val[1], write_net_dic_callback);
		if(rc != 0)
		{
			ERROR_DEBUG("edriver_set_acceleration() failed nodeid:%s", nodeid);
			return NET_WRI_FAIL;
		}
		break;
	case SERVO_3:
		acceleration_val[2] = acceleration;
		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_3, 0x6083, 1, 4, uint32, &acceleration_val[2], write_net_dic_callback);
		if(rc != 0)
		{
			ERROR_DEBUG("edriver_set_acceleration() failed nodeid:%s", nodeid);
			return NET_WRI_FAIL;
		}
		break;
	default:
		break;
	}
	return rc;
}

//	函数功能： 设置减速度
rt_int32_t edriver_set_deceleration(rt_uint8_t nodeid, rt_uint32_t  deceleration)
{
	rt_int32_t	rc = 0;
	switch (nodeid)
	{
	case SERVO_1:
		deceleration_val[0] = deceleration;
		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_1, 0x6084, 0, 4, uint32, &deceleration_val[0], write_net_dic_callback);
		if(rc != 0)
		{
			ERROR_DEBUG("edriver_set_deceleration() failed nodeid:%s", nodeid);
			return NET_WRI_FAIL;
		}
		break;
	case SERVO_2:
		deceleration_val[1] = deceleration;
		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_2, 0x6084, 0, 4, uint32, &deceleration_val[1], write_net_dic_callback);
		if(rc != 0)
		{
			ERROR_DEBUG("edriver_set_deceleration() failed nodeid:%s", nodeid);
			return NET_WRI_FAIL;
		}
		break;
	case SERVO_3:
		deceleration_val[2] = deceleration;
		rc = (rt_int32_t)WriteDicCallBack(eDriver.od_Data, SERVO_3, 0x6084, 0, 4, uint32, &deceleration_val[2], write_net_dic_callback);
		if(rc != 0)
		{
			ERROR_DEBUG("edriver_set_deceleration() failed nodeid:%s", nodeid);
			return NET_WRI_FAIL;
		}
		break;	
	default:
		break;
	}

	return rc;
}

//	复位
void edriver_position_reset(rt_uint8_t nodeid)
{
	rt_int32_t	reset_position_val = 0;
	if(nodeid == SERVO_1)
	{
		WriteDicCallBack(eDriver.od_Data, nodeid, 0x607A, 0, 4, int32, &reset_position_val, write_net_dic_callback);
		EDRIVER_MDELAY(5);
	}
	else if(nodeid == SERVO_2)
	{
		WriteDicCallBack(eDriver.od_Data, nodeid, 0x607A, 0, 4, int32, &reset_position_val, write_net_dic_callback);
		EDRIVER_MDELAY(5);
	}
	else if(nodeid == SERVO_3)
	{
		WriteDicCallBack(eDriver.od_Data, nodeid, 0x607A, 0, 4, int32, &reset_position_val, write_net_dic_callback);
		EDRIVER_MDELAY(5);
	}
	else
	{
		ERROR_DEBUG("edriver_driver_reset() unknown nodeid: %d \n", nodeid);
	} 
}

void edriver_all_servo_reset()
{
	edriver_position_reset(SERVO_1);
	edriver_position_reset(SERVO_2);
	edriver_position_reset(SERVO_3);
}

//	速度归零
void edriver_speed_reset(rt_uint8_t nodeid)
{
	rt_int32_t	reset_speed_val = 0;
	if(nodeid == SERVO_1)
	{
		WriteDicCallBack(eDriver.od_Data, nodeid, 0x60FF, 1, 4, int32, &reset_speed_val, write_net_dic_callback);
		EDRIVER_MDELAY(1000);
	}
	else if(nodeid == SERVO_2)
	{
		WriteDicCallBack(eDriver.od_Data, nodeid, 0x60FF, 1, 4, int32, &reset_speed_val, write_net_dic_callback);
		EDRIVER_MDELAY(1000);
	}
	else if(nodeid == SERVO_3)
	{
		WriteDicCallBack(eDriver.od_Data, nodeid, 0x60FF, 1, 4, int32, &reset_speed_val, write_net_dic_callback);
		EDRIVER_MDELAY(1000);
	}
	else
	{
		ERROR_DEBUG("edriver_speed_reset() unknown nodeid: %d \n", nodeid);
	} 

}



///////////////////////////////////////////////////////////////////////////////////
//初始化设备与canopen相关的回调函数
void master402_heartbeatError(CO_Data* d, UNS8 heartbeatID)
{
	INFO_DEBUG("heartbeatError %d\n", heartbeatID);
}

void master402_initialisation(CO_Data* d)
{
	INFO_DEBUG("canfestival enter initialisation state\n");
	//sca_init();
	
}

void master402_preOperational(CO_Data* d)
{
	rt_thread_t tid;
	INFO_DEBUG("canfestival enter preOperational state\n");
	tid = rt_thread_create("co_cfg", canopen_start_thread_entry, RT_NULL, 1024, 12, 2);
	if(tid == RT_NULL)
	{
		ERROR_DEBUG("canfestival config thread start failed!\n");
	}
	else
	{
		rt_thread_startup(tid);
	}
}

void master402_operational(CO_Data* d)
{
	INFO_DEBUG("canfestival enter operational state\n");
}

void master402_stopped(CO_Data* d)
{
	INFO_DEBUG("canfestival enter stop state\n");
}

void master402_post_sync(CO_Data* d)
{
	INFO_DEBUG("master402_post_sync :  start -> \n");
	//edriver_print_60XX_info();
}

void master402_post_TPDO(CO_Data* d)
{

}

void master402_storeODSubIndex(CO_Data* d, UNS16 wIndex, UNS8 bSubindex)
{
	/*TODO : 
	 * - call getODEntry for index and subindex, 
	 * - save content to file, database, flash, nvram, ...
	 * 
	 * To ease flash organisation, index of variable to store
	 * can be established by scanning d->objdict[d->ObjdictSize]
	 * for variables to store.
	 * 
	 * */
	INFO_DEBUG("storeODSubIndex : %4.4x %2.2x\n", wIndex,  bSubindex);
}

void master402_post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg, const UNS8 errSpec[5])
{
	INFO_DEBUG("received EMCY message. Node: %2.2x  ErrorCode: %4.4x  ErrorRegister: %2.2x\n", nodeID, errCode, errReg);
}




/////////////////////////////////////////////////////////////////////////////////----以上为零差云控代码段



///////////////////////////////////////----------------infoos
//	-----test
//	uint8_t buf[6] = {0x92,0x83,0x54,0x33,0x78,0x23};
//	test_canTransmit(sca1, buf, 6);
//	-----test
